/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  State machine
 */


#include "stateMachine.h"

void setParams()
{
    ROS_INFO("Setting parameters to server");
    ros::NodeHandle nh;

    // for debugging
    nh.setParam("state_machine_debug_on",                    0);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "stateMachine");
    ros::NodeHandle nodeHandle;
    setParams();

    StateMachine sm(nodeHandle);

    ros::Rate loop_rate(100);

	/* BEFORE STARTING ANY EXECUTION IT MIGHT BE
	 * GOOD TO CHECK THAT EVERY NODE IS UP AND RUNNING
	 *
	 * THIS CAN BE DONE FOR EXAMPLE BY CALLING SOME SERVICE
	 * FROM EVERY NODE -> IF THEY REPLY TO SERVICE, THEY ARE 
	 * UP AND RUNNING
     * (will be implemented through testNodes func)
	 */

    sm.start();

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
}

StateMachine::StateMachine(ros::NodeHandle& nh)
{
    mNodeHandle = nh;

    //FOR MotionCtrl
    mMotionCtrlApproachPub = mNodeHandle.advertise<group6::motion_ctrl>("goToPoint", 10); // Tell how to move, so this initializes the values for movement
    mApproachClient = mNodeHandle.serviceClient<group6::approach>("approach"); //Service for the actual movement

    // FOR PathPlanner
    mGoToGoalClient = mNodeHandle.serviceClient<group6::goToNextGoal>("goToNextGoal");

    // services for servoCtrl
    ptuServoClient = mNodeHandle.serviceClient<group6::transformCameraServo>("transform_camera_servo");
    manipServoClient = mNodeHandle.serviceClient<group6::transformManipulatorServo>("transform_manipulator_servo");

    // initialise and test systems
    updateState(STATE_INITIALIZING);

}

void StateMachine::start()
{

    bool everythingOK = true;       // use everythingOK for the command results and act accordingly if something goes wrong.
                                    // allways make sure that the program can continue even if an action fails
    bool programFinished = false;
    int target = -1;

    ROS_INFO("STARTING ACTION!");

	// move servos to initial position

    while(!programFinished)
    {
        if (currentState == STATE_INITIALIZING) 
        {
            everythingOK = initCameraAttitude();
            if (everythingOK)
            {                
                everythingOK = hoverManipulator();
                if (everythingOK)
                {
                    everythingOK = release();
                    if (everythingOK) 
                    {
                        updateState(STATE_RANDOM_SEARCH);
                    }
                }
            }
        }
	      else if (currentState == STATE_RANDOM_SEARCH)
	      {
		      ROS_INFO("ORDER: RANDOM SEARCH");
		      int responseStatus = commandToPathPlanner(PathPlanner::RANDOM_SEARCH);
		      if (responseStatus == PathPlanner::SUCCEED)
		      {
			      updateState(STATE_MOVE_TO_TARGET);
		      }
		      else if (responseStatus == PathPlanner::NOT_SUCCEED)
		      {
			      updateState(STATE_BACKOFF_RANDOM_SEARCH);
		      }
	      }
        else if (currentState == STATE_MOVE_TO_TARGET)
        {
            ROS_INFO("ORDER: MOVING TO TARGET");
            target = PathPlanner::TOY;
            int responseStatus = commandToPathPlanner(PathPlanner::TOY);
            if (responseStatus == PathPlanner::SUCCEED)
            {
                updateState(STATE_APPROACH_TARGET);
            }
            else if (responseStatus == PathPlanner::NO_TOYS_LEFT)
            {
                // no toy found, restart random search
                updateState(STATE_RANDOM_SEARCH);
            }
            else
            {
                // error mode
                ROS_ERROR("ERROR! Was not able to move to target, performing emergency backoff");
                updateState(STATE_EMERGENCY_BACKOFF);
            }
        } 
        else if (currentState == STATE_APPROACH_TARGET)
        {
            ROS_INFO("ORDER: APPROACHING TARGET");
            everythingOK = approachCommandToMotionCtrl(PathPlanner::TOY);
            if (everythingOK)
            {
                updateState(STATE_ACQUIRE_TARGET);
            }
            else
            {
                // error mode: skip this toy and go to the next
                ROS_ERROR("ERROR! Was not able to approach target, skipping target instead");
                updateState(STATE_MOVE_TO_TARGET);
            }
        }
        else if (currentState == STATE_ACQUIRE_TARGET)
        {
            ROS_INFO("ORDER: ACQUIRING TARGET");
            everythingOK = lowerManipulator();
            if (everythingOK)
            {
                everythingOK = grab();
                if (everythingOK)
                {
		    ros::Rate sleepBeforeLift(1);
		    sleepBeforeLift.sleep();
			
                    everythingOK = raiseManipulator();
                    if (everythingOK)
                    {
                        updateState(STATE_BACKOFF_FROM_TOY);
                    }
                }
            }
        }
        else if (currentState == STATE_BACKOFF_FROM_TOY)
        {
            everythingOK = escapeCommandToMotionCtrl(MotionCtrl::ESCAPE_FROM_TOY);
            if (everythingOK)
            {
                updateState(STATE_MOVE_TO_BASKET);
            }        
        }
        else if (currentState == STATE_MOVE_TO_BASKET)
        {
            ROS_INFO("ORDER: MOVING TO BASKET");
            target = PathPlanner::BASKET_1;
            int responseStatus = commandToPathPlanner(PathPlanner::BASKET_1);
            if (responseStatus == PathPlanner::SUCCEED)
            {
                updateState(STATE_APPROACH_BASKET);
            } else {
                ROS_ERROR("Error! Was not able to move to basket");
                updateState(STATE_EMERGENCY_BACKOFF);
            }
        }
        else if (currentState == STATE_APPROACH_BASKET)
        {
            ROS_INFO("ORDER: APPROACHING BASKET");
            // raise manipulator first before approaching otherwise the lowered manipulator
            // collides with the basket
            everythingOK = raiseManipulator();
            if (everythingOK)
            {
                everythingOK = approachCommandToMotionCtrl(PathPlanner::BASKET_1);
                if (everythingOK)
                {
                    everythingOK = release();
                    if (everythingOK)
                    {
                        updateState(STATE_BACKOFF_FROM_BASKET);
                    }
                } else {                    
                    ROS_ERROR("ERROR! Was not able to approach basket");
                    updateState(STATE_EMERGENCY_BACKOFF);
                }  
            }
        }
        else if (currentState == STATE_BACKOFF_FROM_BASKET)
        {
            ROS_INFO("ORDER: BACKING OFF FROM BASKET");
		        everythingOK = escapeCommandToMotionCtrl(MotionCtrl::ESCAPE_FROM_BASKET);
		        if (everythingOK) {
        	    everythingOK = hoverManipulator();
        	    if (everythingOK)
        	    {
        	        updateState(STATE_MOVE_TO_TARGET);
        	    }
		        }
        }
	      else if (currentState == STATE_BACKOFF_RANDOM_SEARCH)
	      {
		      ROS_INFO("ORDER: BACKOFF FOR RANDOM SEARCH");
		      everythingOK = escapeCommandToMotionCtrl(MotionCtrl::BACKOFF);
		      if (everythingOK) {
			      updateState(STATE_RANDOM_SEARCH);
		      }
	      }
        else if (currentState == STATE_FINISHED)
        {
            ROS_INFO("ORDER: COMPLETE. GO TO NEXT TASK");
            updateState(STATE_MOVE_TO_TARGET);
        }
        else if (currentState == STATE_EMERGENCY_BACKOFF)
        {
            ROS_INFO("ORDER: EMERGENCY BACKOFF");
            everythingOK = escapeCommandToMotionCtrl(MotionCtrl::BACKOFF);
            if (everythingOK)
            {
                if (target == PathPlanner::TOY)
                {
                    updateState(STATE_MOVE_TO_TARGET);
                }
                else if (target == PathPlanner::BASKET_1)
                {
                    updateState(STATE_MOVE_TO_BASKET);
                }
            }
        }
        else if (currentState == STATE_MISSION_COMPLETE)
        {
            ROS_INFO("Mission completed.");
            // do something fancy like driving back to start
		programFinished=true;
        }
    }
}

int StateMachine::commandToPathPlanner(PathPlanner::currentDestination target)
{
    ROS_INFO("-----------------");
    if(target == PathPlanner::TOY)
    {
        ROS_INFO("GOING TO THE TOY");
        mServiceToPathPlanner.request.question = PathPlanner::TOY;
    }
    else if(target == PathPlanner::BASKET_1)
    {
        ROS_INFO("GOING TO THE BASKET 1");
        mServiceToPathPlanner.request.question = PathPlanner::BASKET_1;
    }
    else if(target == PathPlanner::BASKET_1)
    {
        ROS_INFO("GOING TO THE BASKET 2");
        mServiceToPathPlanner.request.question = PathPlanner::BASKET_2;
    }
    else if(target == PathPlanner::RANDOM_SEARCH)
    {
        ROS_INFO("PERFORMING RANDOM SEARCH");
        mServiceToPathPlanner.request.question = PathPlanner::RANDOM_SEARCH;
    }
    else
    {
        ROS_ERROR("BAD TARGET GOAL!");
        return -1;
    }

    if(mGoToGoalClient.call(mServiceToPathPlanner) &&
            mServiceToPathPlanner.response.result == PathPlanner::SUCCEED)
    {
        ROS_INFO("SUCCESS commandToPathPlanner!");
        ROS_INFO("-----------------");

    }
    else
    {
        ROS_INFO("FAILED commandToPathPlanner!");
        ROS_INFO("-----------------");
    }
    return mServiceToPathPlanner.response.result;
}

/** 
 * @param target: The target as defined in PathPlanner.currentDestination
 */
bool StateMachine::approachCommandToMotionCtrl(PathPlanner::currentDestination target)
{
    ROS_INFO("-----------------");
    if (target == PathPlanner::BASKET_1)
    {
        mServiceToMotionCtrl.request.question = MotionCtrl::APPROACH_BASKET1;
    }
    else if (target == PathPlanner::BASKET_2)
    {
        mServiceToMotionCtrl.request.question = MotionCtrl::APPROACH_BASKET2;
    }
    else
    {
        mServiceToMotionCtrl.request.question = MotionCtrl::APPROACH;
    }

    ROS_INFO("APPROACHING");
    if(mApproachClient.call(mServiceToMotionCtrl))
    {
        int result = mServiceToMotionCtrl.response.result;
        if (result == MotionCtrl::SUCCEED)
        {
            return true;
        }
        else if (result == MotionCtrl::NOT_SUCCEED)
        {
            ROS_ERROR("ERROR! General error in approach call!");
            return false;   
        }
        else if (result == MotionCtrl::ERROR_TOY_NOT_FOUND)
        {
            ROS_ERROR("ERROR! Toy not found!");
            return false;   
        }
        ROS_INFO("FAILED approachCommandToMotionCtrl!");
        return false;
    } else {
        return false;
    }

    ROS_INFO("SUCCESS approachCommandToMotionCtrl!");
    ROS_INFO("-----------------");
    return true;
}

bool StateMachine::lowerManipulator()
{
    //@todo: implement sleep in between tries
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformManipulator(ServoCtrl::SERVO_MANIP, ServoCtrl::LOWER_MANIP);
        raiseTries++;
        rosSleep();
    }
    return success;
}

bool StateMachine::hoverManipulator()
{
    //@todo: implement sleep in between tries
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformManipulator(ServoCtrl::SERVO_MANIP, ServoCtrl::HOVER_MANIP);
        raiseTries++;
        rosSleep();
    }
    return success;
}

bool StateMachine::raiseManipulator()
{
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformManipulator(ServoCtrl::SERVO_MANIP, ServoCtrl::RAISE_MANIP);
        raiseTries++;
        rosSleep();
    }
    return success;
}

bool StateMachine::grab()
{
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformManipulator(ServoCtrl::SERVO_MANIP, ServoCtrl::GRAB);
        raiseTries++;
        rosSleep();
    }
    return success;
}

bool StateMachine::release()
{
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformManipulator(ServoCtrl::SERVO_MANIP, ServoCtrl::RELEASE);
        raiseTries++;
        rosSleep();
    }
    return success;
}

bool StateMachine::initCameraAttitude()
{    
    int raiseTries = 0;
    bool success = false;
    while (!success && raiseTries < MAX_RAISE_TRIES)
    {
        success = transformCamera(ServoCtrl::SERVO_PTU, ServoCtrl::TILT_CAMERA, 132);
        raiseTries++;
        rosSleep();
    }
    
    if (success) {
        raiseTries = 0;
        success = false;
        while (!success && raiseTries < MAX_RAISE_TRIES)
        {
            success = transformCamera(ServoCtrl::SERVO_PTU, ServoCtrl::PAN_CAMERA, 88);
            raiseTries++;
            rosSleep();
        }
    }

    return success;
}

bool StateMachine::transformCamera(int servoType, int action, int rotation)
{
    if (servoType == ServoCtrl::SERVO_PTU)
    {
        mTransformCamera.request.action = action;
        mTransformCamera.request.rotation = rotation;

        if (!ptuServoClient.call(mTransformCamera) ||
            !mTransformCamera.response.success)
        {

            ROS_INFO("FAILED transformCamera!");
            ROS_INFO("-----------------");
            return false;
        }

        ROS_INFO("SUCCESS transformCamera!");
        ROS_INFO("-----------------");
        return true;
    }
    else
    {
        ROS_INFO("THIS SERVO DOES NOT EXIST!");
        ROS_INFO("-----------------");
        return false;
    }
}

bool StateMachine::transformManipulator(int servoType, int action)
{
    if (servoType == ServoCtrl::SERVO_MANIP)
    {
        mTransformManipulator.request.action = action;

        if (!manipServoClient.call(mTransformManipulator) || 
            mTransformManipulator.response.state != ServoCtrl::SERVO_SUCCESS) 
        {
            ROS_INFO("FAILED transformManipulator!");
            ROS_INFO("-----------------");
            return false;
        }

        ROS_INFO("SUCCESS transformManipulator!");
        ROS_INFO("-----------------");
        return true;
    }
    else
    {
        ROS_INFO("THIS SERVO DOES NOT EXIST!");
        ROS_INFO("-----------------");
        return false;
    }
}

void StateMachine::updateState(states newState)
{
    currentState = newState;

    if(!mNodeHandle.getParam("state_machine_debug_on", debugON))
    {
        ROS_INFO("Failed to get debug parameter");
    }
    else if(debugON != 0)
    {
        ROS_INFO("Press enter to continue to next state, debug: %f", debugON);
        getchar();
    }


    ROS_INFO("-------------");
    //ROS_INFO("CONTINUING TO STATE " + stateDescriptions[newState]);
    ROS_INFO("CONTINUING TO NEXT STATE");
    rosSleep();
    ROS_INFO("-------------");
}

bool StateMachine::escapeCommandToMotionCtrl(int target)
{
    ROS_INFO("-----------------");
    mServiceToMotionCtrl.request.question = target;
    ROS_INFO("ESCAPING FROM THE TARGET");
    if(!mApproachClient.call(mServiceToMotionCtrl) ||
            mServiceToMotionCtrl.response.result != MotionCtrl::SUCCEED)
    {
        ROS_INFO("FAILED!");
        return false;
    }
    ROS_INFO("SUCCEED!");
    ROS_INFO("-----------------");
    return true;
}

bool StateMachine::testNodes() {
    // implement this for final /rian
    return true;
}

void StateMachine::rosSleep()
{
    ros::Rate manipulate(2);
    manipulate.sleep();
}
